#ifndef CONTROLLER_H_INCLUDED
#define CONTROLLER_H_INCLUDED

#include <Aria.h>
#include <string>
#include "VisionControllerBW.h"
#include "VisionController.h"
#include "FollowMarkerActionGroup.h"
#include "GoCurveActionGroup.h"
#include "SearchActionGroup.h"
#include "ObstacleAvoidanceActionGroup.h"
#include "SearchContinuoslyGroup.h"
#include <opencv2/opencv.hpp>

using namespace std;
//Thread che effettua il controllo del robot
class RobotController {

private:
	//Stati in cui può trovarsi l'agente
	enum State {
		FOLLOW_MARKER, FOLLOW_TRAJ, SEARCH, AVOIDANCE
	};

	/*
	 * Parametri che cambiano dinamicamente e sono passati per riferimento alle azioni
	 */
	int direction;
	uint16_t distance;

	const static int leftDirection = 1;
	const static int rightDirection = -1;

	/*
	 * Parametri di VisionController
	 */
	int maxGhostFrames;
	float stackedBoxHorizontalTolerance;
	float stackedBoxVerticalTolerance;
	//distanza dal centro per considerare fuoriuscita marker
	int oldEpsilonFromCenter;
	int epsilonFromCenter;
	int strictEpsilon;
	int largeEpsilon;
	//distanza dal centro da considerare nella quale stabilizzare l'orientamento del robot rispetto al marker
	int epsilonStabilization;

	/*
	 * 	 Parametri della Search
	 *
	 */
	int minRotVelToConsiderStoppedTheRobot;
	int searchRotVel;
	double searchTimeInterval;
	double searchHeadingInterval;
	int searchRotDecel;

	/*
	 * Parametri della FollowMarker
	 */
	int distanceSetPoint;
	int followRotVel;
	int followTransVel;
	int followVMax, followVMin, followDMax;
	int weakDecel, strongDecel;
	int fastRotVel, slowRotVel;
	int fastRotAcc, slowRotAcc;
	double stoppedTime;
	double stoppedStartTime;
	int lastValidDirection;

	/*
	 * Parametri della FollowTraj
	 */
	int curveRotVel;
	int curveTransVel;
	double curveTurningTime;
	int curveRotDecel;
	uint16_t distanceToCurve;

	/**
	 * Parametri obstacle avoidance
	 */
	uint16_t minDistanceToObstacle;
	int obstacleRotVel;
	uint16_t obstacleAvoidingDistanceToTravel;
	int obstacleTransVel;
	double obstacleTurningTime;//in secondi
	uint16_t minDiffMarkerFromObstacle;

	uint16_t lastValidDistance;
	float safetyFactorInCaseOfObstacle;

	double distanceToAdvanceInCaseOfObstacle;

	//Gruppo di azioni da attivare quando ci si trova nello stato NO_TARGET
	FollowMarkerActionGroup* followMarkerState;
	GoCurveActionGroup* goCurveState;
	SearchActionGroup* searchState;
	ObstacleAvoidanceActionGroup* obstacleAvoidanceState;
	SearchContinuoslyGroup* searchContinuoslyState;

	//Parte del controllore deputata alla visione
	VisionControllerBW* vision;
	//Robot
	ArRobot* robot;
	//Stato Attuale	e precedente
	State myState;

	void initRobotParameters();
	/**
	 * restituisce una label che rappresenta la posiz. dell'ostacolo
	 * es.:
	 * -2 ostacolo a sinistra
	 * -1 ostacolo a centro-sinistra
	 * 0 nessun ostacolo
	 * 1 ostacolo a centro destra
	 * 2 ostacolo a destra
	 */
	int checkObstacles(vector<uint16_t>* left, vector<uint16_t>* right);
	uint16_t getMarkerDistanceFromObstacle(vector<uint16_t>* right,vector<uint16_t>* left,int obstacleLabel,
			uint16_t markerDistance);
	uint16_t getMinValue(uint16_t a,uint16_t b);

	//Variabili e metodi per la gestione del thread
	volatile bool stop_request;
	volatile bool running;
	pthread_t thread;
	//Flusso di esecuzione del thread
	void do_work();
	static void* start(void *obj) {
		reinterpret_cast<RobotController*>(obj)->do_work();
	}

//	void updateState();

public:
	RobotController(ArRobot*);
	~RobotController();
	void stop();
	void run();

};

#endif
